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PROBLEM  DESCRIPTION 

The  problem  presented  here  is  part  of  the  investigations  conducted  for  the 
design  of  the  data  acquisition  and  decision  making  system  for  an  unmanned 
exploratory  platform.  As  we  know,  it  is  necessary  for  the  platform  to  be 
equipped  with  a  data  acquisition  and  a  decision  making  system. 

Usually,  a  laser  range  finder  is  chosen  as  a  main  sensing  device  which  can 
determine  the  distance  to  a  point  within  a  certain  accuracy.  With  reference  to 
Figure  1,  the  terrain  is  scanned  by  varying  the  azimuth  angle,  e^-,  and  the  ele¬ 
vation  angle,  /3^,  of  the  laser  beam  in  a  discrete  fashion.  In  general,  the 
measurements  are  then  available  in  the  form  of  a  "range  matrix"  as 

M  =  {m,j}  (1) 

where  i  3  i,2,...,N,  correspond  to  the  elevation  angle, 
j  3  1,2, ...,L,  correspond  to  the  azimuth  angle,  8j 

The  range  matrix  M  above  describes  a  certain  scanned  area  of  the  ground  in 
front  of  the  mobile  robot  platform  and  can  be  used  to  detect  edges  of  discrete 
objects  or  to  estimate  the  slopes  of  the  terrain.  This  is  a  two-dimensional 
grid.  In  our  study  for  simplicity,  a  one-dimensional  case  is  first  considered, 
that  is,  change  fij  for  a  fixed  0j.  Thus,  we  have  observation  data  m^ 
corresponding  to  fij,  from  which  we  can  estimate  the  in-path  slopes  of  the  range. 

Generally  speaking,  both  /J-j  and  m^  are  subject  to  random  errors  such  that 


fii  =  «i  +  Vi  (2) 

®i(0i)  *  f(Pi)  Mi  (3) 

where 

(i  are  known  knots;  (4a) 

f(C)  is  an  unknown  function;  (4b) 

fij  are  stochastic  intervals;  (5a) 

mi (Pi)  are  measurements  corrupted  by  noise;  (5b) 
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v-j  and  are  random  errors  with  zero  means,  for  all  1  » 

Equations  (2)  and  (3)  Imply  that  both  Independent  and  dependent  variables 
are  corrupted  by  random  noise,  which  may  make  the  problem  more  complicated,  as 
shown  In  Figure  2. 

With  the  nodal  described  above,  we  are  now  able  to  formulate  the  problem. 


OBJECTIVE  FUNCTION  FORMULATION 


Referring  to  Figure  3,  It  Is  apparent  that  we  cannot  use  the  method  of 
Interpolation  directly  to  approximate  the  unknown  function  f  since  the  fi-y  are 
stochastic.  Instead  of  this,  we  would  like  to  estimate  f  by  an  approximating 
function  h;  our  objective  Is  to  determine  the  values  of  function  h  and  Its  first 
derivatives  due  to  random  angle  error  v^. 

The  following  relations  can  be  derived  by  using  Taylor's  series  expansion 


at 


^1  = 


df(5i) 

f(/Ji)  *  f(?i)  ♦  Vi  ♦  o(vi) 

at 


(6) 


where  o(v)  represents  the  higher  order  term  of  v.  Using  the  first-order 
approximation  yields: 


f(ei)  a  f(«i)  + 


df(€i) 
"di" 


Vi 


(7) 


Similarly,  for  the  approximating  function  h  to  estimate  the  unknown  function  f, 
we  have 


h(/5i)  »  h(5i)  ♦ 


dh{5i) 

■"dr" 


From  the  equations  above,  one  obtains 

A  dh({i) 

Ai  -  h(/Ji)  -  h(?i)  =  -----  (Vi) 

dc 


(8) 


(9) 


The  stochastic  difference  Xi  for  the  approximating  function  h  should  be  as  small 
as  possible  In  this  problem. 
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We  will  attempt  to  determine  a  function  approximation  h  with  a  compromise 
between  the  following  objectives: 

1.  The  measurement  value  m^  should  be  fitted  close  enough. 

2.  The  approximating  function  h  should  be  smooth  enough  in  the  sense  that 
the  discontinuities  in  its  second  derivative  are  as  small  as  possible. 

Now  in  order  to  be  able  to  formulate  this  criterion  mathematically,  we  need 
to  find  some  measure  of  smoothness  and  some  measure  of  closeness  of  fit.  To  be 
more  specific,  suppose  that  L  is  a  linear  space  of  "smooth"  functions  and  that 
is  a  functional  defined  on  L  which  measures  how  well  a  function  fits  the 
data.  Suppose  in  addition  that  J3  is  a  functional  defined  on  L  which  measures 
the  smoothness  of  an  element  on  L,  then  the  smoothing  problem  is  the  following: 
find  s  £  L,  such  that 

J(s)  =  inf  J(h)  or  J(s)  <  J(h)  (10) 

h  £  L 

where 

J(h)  =  Jc(h)  +  Js{h)  (11) 

The  Measure  of  Closeness 

From  Figure  3,  in  order  to  fit  data  (v^,m.j)  as  closely  as  possible,  we  can 
simply  take  the  sum  of  squared  errors  from  Eqs.  (3)  and  (9)  as  a  measure  of 
closeness  of  fit  J^,  that  is 

N  dh  dh 

Jc  =  Z  (Vi  ;--)TRi’(vi  --)  ♦ 

ial  a?  df 

N 

+  Z  [h(«i)  -  mi(5i)]'^Ri’[h(5i)  -  •iii(«i)]  (12) 

i=l 

where  from  Eq.  (3) 

Ri  *  ECuiUiT]  (13) 
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Here,  and  R,  are  weighing  matrices  which  reflect  our  degree  of  con¬ 
fidence  in  the  data  m^-  and  v-;.  For  convenience,  it  is  assumed  that  R.j  and  R-j 
are  constants  for  all  i  = 

From  Eq.  (9),  the  error  due  to  the  random  noise  v-j  can  be  written  as 


dh(5i) 

'~di“' 


Vi 


(14) 


It  is  assumed  that  the  random  noise  v,-  is  a  certain  stochastic  process  with  zero 
mean  as  studied  in  the  next  section.  Thus,  the  error  covariance  matrix  for  X^- 
is 


ECXiXiT] 


dh(5i) 

"di"' 


,  dh({i) 

E[ViViT][--.-]T 

dc 


(15) 


The  Measure  of  Smoothness 

From  the  viewpoint  of  the  approximation  theory,  the  spline  function  pro¬ 
vides  a  means  of  optimally  reconstructing  an  unknown  function  f  such  that 

€n  Ah 

L  [s*(«)]*d«  <  /  th*{0]*d?  *  Jg  (16) 

<1  «1 

where  s  is  a  spline  function,  s  e  C*,  and  h  is  an  approximation  function  of  the 
unknown  function  f  and  h  e  C*.  Thus,  s(?)  is  the  function,  among  those 
satisfying  some  constraints,  that  is  "smoothest"  is  the  sense  of  Eq.  (16). 

To  be  more  convenient  and  simpler,  in  our  study  we  will  use  the  piecewise 

cubic  spline  s  c  C*.  Then  Eq.  (16)  can  be  rewritten  as 


N  5i 

Jg  =  P  Z  L  [h(0]*d5 
i*l  €i-l 


(17) 


In  sumiary,  substituting  Eqs.  (12)  and  (17)  into  Eq.  (11)  yields  an  objective 
functional  0 


N 

J  =  E  [h(?i)  -  mi]TR::’[h(4i)  -  mi] 
i“l 

N  dh  dh  N  fi 

♦  E  (--  Vi)'RiM:-  Vi)  ♦  p  E  [h(?)]*d?  (18) 

i=l  df  i*l  ?i-l 

where  p  >  0  is  a  smoothing  parameter. 


STOCHASTIC  EXPECTATION 

Since  Vi  is  a  stochastic  variable,  we  use  the  expected  value  of  J,  E[J],  as 
the  functional  to  be  minimized,  i.e., 

N 

E[J]  .  E{  E  [h(?i)  -  i«i]TR;’[h(ei)  -  mil 
i»l 

^  dh  T*-,  dh  ^  ,^i  , 

♦  E  (--  Vi)'Ri’(--  Vi)  +  p  E  /  [h(?)]*d5}  (19) 

i=l  d?  d{  i-2  ^i-i 

where  h  is  a  set  of  any  piecewise  cubic  Hermite  polynomials  and  h  e  C'. 

In  order  to  obtain  the  solution  to  Eq.  (19),  the  following  algebraic  manip¬ 
ulations  for  Eq.  (19)  are  necessary: 

Define  hi  =  h(?i)  ,  hi  =  h(?i) 


N 


T«T-_,. 

n  *  I 


N 


E[J]  *  E  [(hi-mi)TRT’  (hi-mi)  +  E(VihiR:f’hiVi)]  ♦  p  E  /[h(5)]*d?  (20) 

i=l  i*2 


Using  the  matrix  identity,  the  second  term  of  the  above  equation  becomes 


E(v{h{R:f’hiVi)  =  Trace  [(hiR;’hi)E(ViVi)] 


and 


T  A 

E(ViVi)  «  o* 


(21) 
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Then, 


T  -1  'T*-,*  T 

E[J]  *  E  (h,-mi)TR,’(h,-m,)  +  Trace[(hiR,-  hT)E(VTV,)] 

i=l 

N  _  •  *_  .  N  ■ 

»  E  [(hT-mi  )TR^’{hi-ini)  +  (a^hi )Tr^ ’  (aih,- ) ]  +  p  E  /  th(5)]*d€  (22) 

i*>l  1=2  €i-l 

As  mentioned  previously,  our  main  purpose  is  to  obtain  the  estimates  of  the 

values  of  the  unknown  function  and  its  first  derivatives  at  grids 

i=l,2,...,N,  i.e.,  h(5^)  and  h(5i).  To  make  the  problem  simpler,  it  is  proposed 

that  the  state  variable  approach  be  used.  Thus  the  optimization  problem  in  Eq. 

(19)  can  be  transformed  into  a  state  estimation  problem,  where  each  state  vector 

x-j  is  defined  as: 

Xi  =  [h(5),h(0]‘r  I  r  (23) 

.  ^1 

Hence,  the  objective  functional  J  becomes 

N  .  -  ^  ^ 

E[J]  =  Z  [(HXi-mi)TR^’(Hxi-mi)  +  (Hx^ )Tr7’ (Hx^ ) ] 
i=l 

N  (3^  •• 

*  P  I  f  (P(f)]«d?  (24) 

i=2  /Si-1 

where 

H  =  [1,0]  ,  H  =  [0,0^]  (25) 

Now  the  key  to  solving  the  problem  in  Eq.  (24)  is  to  evaluate  the  smoothing 
integral  so  as  to  obtain  an  expression  in  terms  of  state  variables. 

Problem  Formulation  for  Two-Dimensional  Grid 

When  the  observation  data  are  noise  corrupted  and  the  underlying  system  is 
unknown,  it  is  proposed  to  approximate  the  original  signal  by  spline  functions 
which  minimize  a  certain  objective  function.  Thus,  from  a  set  of  discrete 
measurements,  m^^j,  corrupted  by  the  white  noise  process,  v-j^j 


6 


nii^^j(|5^,<7j)  =  ,  i  -  j  *  1 ,2 , .  . .  ,H  (26) 

with  angle  errors 

Pi  =  ^.j  +  Vi  and  0j  =  hj  +  Uj  (27) 

The  original  two-dimensional  signal  f(5,n)  defined  in  the  region  of  (?,n)  is 
approximated  by  a  spline  function  s{^,Tj)  which  minimizes  the  following  objective 
function; 

J  =  Jc  +  Js  (28) 

where 

M  N 

Jc  =  E  E  [s(Pi,ej)  -  mi,j]TRi;j[s(Pi,ej)  -  mi,j] 
j=l  i=l 


M  N 

+  E  E  [s(Pi,0j)  -  s(^i»ilj)]  Rij[s(Pi,0j)  -  s(^i,nj)]  (29) 

j=l  i=l 


and 


,>1M  .^N 

Js  =  PlI  L  z(?,n)d?dn] 

Hi  h 


(30) 


a  . 

where  p  >  0  is  the  smoothing  parameter;  Ri^j  =  EtPijPij']  is  the  observation 
error  covariance;  and  z(?,r})  is  a  certain  smoothness  measure  of  s(^,n)  at  (^,n). 
The  following  relation  can  be  derived  by  using  Taylor's  series  expansion  at 

(8i,8j): 

as  as 

s(/3i,0j)  -  s(5i,rjj)  =  --  (5i.nj)  Vi  +  --  {ei,nj)uj  +  o(vi)o{Uj)  (31) 


Since  Vi  and  uj  are  stochastic  variables,  we  will  use  the  expected  value  of  Jc» 
E(Jc)  as  the  functional  to  be  minimized. 

Def i ne 


as 

S5(i,j)  =  "  (firOj) 


as 

Sn(iJ)  =  --  (5i.nj) 


(32) 


(33) 
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We  have 


M  N 

E[Jc]  =  E  E  {[s(i,j)-mij]TR::][s(i,j)-inij] 

j=l  i=l 


+  tOjS„(i,j)lTRi]tQjSp{i,j)]}  (34) 

where 

T 

a,*  *  E[viVi]  (35) 


and 

T 

Qj*  =  E[UjUj]  (36) 

Choice  of  An  Approxiaatinq  Function 


In  this  report,  we  are  interested  in  obtaining  smoothed  estimates  of  func¬ 


tion  values  and  the  first  derivatives  in  both  {  and  rj  directions.  Here,  we  prO' 


pose  to  restrict  our  approximating  functions  to  piecewise  bicubic  Hermite 


polynomials  which  have  continuous  first  derivatives  in  both  {  and  r?  directions. 


Let  us  define 

A  ds  as  a*s  T 

^i/j  ■  [s(4,n)  r  —  (?»1?)  .  —  (?»Il)  »  - (5rh)],c.  _.v  (37) 

H  9ti  a^an 


for  i  =  1,2,...,N  and  j  =  1,2,...,M.  Then  a  piecewise  bicubic  Hermite  polyno¬ 


mial  is  completely  defined  by  i  =  1,2,...,M  as  follows: 

s(5,n)  =  Si j(5,n)  ,  for  =  5  =  (38) 

and 


hj  s  n  a  'Ij  +  l  (39) 

Thus,  Si^j({,i7)  can  be  written  in  terms  of  the  basis  functions  (|i  and  »|»,  which 
are  the  bicubic  Hermite  polynomials  (spline  functions),  as  follows: 
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T 


Si , j(5fn) 


1  1 
=  r  E 
J>0  asO 


*  *i+l,j+a 


(40) 


Then  Eq.  (34)  becomes 

M  N 

*  E  E  ([Hxi^j  -  *1 , jl^^i j[Hxi ^ j  -  mij] 
j=l  1*1 

+  [^iXi j]^Ri j [HiXi j] 


where 

+  [HjXi j]^Ri j [HjXi j] } 

(41) 

H  *  [1  0  0  0] 

(42) 

i 

Hi  =  [0  <Ti  0  0] 

(43) 

o 

cx 

o 

II 

<  X 

(44) 

Choice  of  the  Smoothness  Measure  z(£.n> 


Here  we  present  three  examples  of  the  smoothness  measures  and  compare  their 
physical  implications. 

1.  Gaussian  curvature:  The  mean  curvature  of  a  surface  at  (?,n)  is  defined 
as: 

(o.5)y*s{?,n)  (45) 

Noting  in  Euler's  theorem  that  the  sum  of  two  curvatures  in  perpendicular  direc¬ 
tions  at  a  point  is  constant,  the  square  of  V*s({,)7)  in  Eq.  (45)  would  be  a 
reasonable  measure  for  the  smoothness  of  a  surface 

3*  3* 

z(?,n)  *  [---  s(5,n)  +  —  s(€,n)]*  (46) 

3?*  3i7* 
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2.  A  variation  from  the  Gaussian  curvature:  With  reference  to  Eq.  (46), 
an  Interesting  case  occurs  when  the  two  principal  curvatures  are  equal  and  of 
the  opposite  sign.  The  mean  curvature  In  this  case  Is  zero.  This  Is  the  so- 


called  "saddle  point"  and  every  surface  element  of  such  a  membrane  Is  "pure 
twist."  An  appropriate  smoothness  measure  would  be  changed  to: 

2(?.n)  *  (---  s(?.n)]*  +  [---  s(?,n)]*  (4?) 

a{*  8n* 

3.  It  Is  suggested  to  use  1 1  V*s(C,il)|  I  *  as  a  smoothness  measure  for  a  sur¬ 
face.  The  physical  interpretation  of  the  quantity  V^s(?,n)  Is  found  in  a  plate 
bending  theory;  an  unloaded  plate  can  bend  only  In  a  blharmonlc  function  u  where 

=  0  (48) 

Smoothing  Integral 

Now  we  need  to  determine  the  function  s({,n)  which  minimizes  the  objective 
function  J  In  Eq.  (28).  It  Is  noted  that  the  smoothing  integral  in  Its  present 
form  gives  difficulties  In  finding  an  explicit  solution.  By  evaluating  the 
Integrals  of  the  derivatives  of  basis  functions  and  applying  some  algebraic 
manipulations,  these  smoothing  Integrals  are  converted  to  quadratic  forms  as 
fol lows: 

M-1  N-1  rjj+i  ?i+i 

Js  -  P  /  /  [2(5»n)]d?db  =  P  E  E  /  [z(5,n)]d?dn 

Hi  ej  j»l  i«i  rij  ei 

M-1  N-1 

*  E  E  (’<1,j^»  ^1+1, j^'  ’^Ifj+l^'  ’*1+l,j+l^)  * 
j=l  1=1 

C  •  (XijT,  Xi+ijT,  Xij+jT,  Xi+i,j+iT)T  (49) 

where  C  Is  a  16  by  16  matrix. 


Now  we  can  put  the  objective  function  together  frow  Eqs.  (28),  (45),  and 


(49)  as 

J  *  ECOg]  ♦ 

H  N 

a  E  E  j]TR^ j [Hx,- ^ j  -  a-jj] 

j-l  i»l 


+  [H-jX-j j]^R-j j [H-jX-j j]  ♦  [HjX.j j]^R^ j [HjX.j j] } 

M-1  N-1 

♦St  [Xi,jT,  Xi+i^jT,  Xij+jT^  ’<i+l,j+l^] 

jal  1*1 

•  C[XijT,  »<1+l,j^'  ^1,j+l'^'  ’<1+l,j+l'^]''’  (50) 

For  the  optimal  values  of  J,  one  can  take  the  partial  derivative  of  J  with 
respect  to  the  state  variables  j,  etc.  and  set  to  zero. 


CONCLUSIONS 

The  effect  of  random  angular  errors  to  the  range  measurement  has  been 
studied  with  the  approximating  function  at  the  random  node  modeled.  The  func¬ 
tional  Is  expressed  first  In  terms  of  the  smooth  functions  and  then  In  terms  of 
state  variables.  An  expected  value  of  this  functional  Is  formulated  so  that  It 
is  In  terms  of  the  covariance  of  the  random  angular  errors.  The  work  has  been 
extended  to  two-dimensional  random  grids  using  bicubic  Hermite  polynomials. 
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Figure  3.  Representation  of  function  due  to  angu1 
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